#include "Group.h"
#include "Entity Manager.h"
#include "Message Dispatcher.h"
#include "GroupStates.h"
#include "Steering Behavior.h"
#include "ExecutionLog.h"
#include "Transformations.h"
#include "GenericKeys.h"
#include "GameWorld.h"
#include "Defines.hpp"

int Group::m_iNextValidID = 0;

double ManhattanDist(Vector2D pos, Vector2D dest)
{
	return (abs(pos.x - dest.x) + abs(pos.y - dest.y));
}

// ================================= Constructor / Destructor ==========================
Group::Group()
{
	m_ID = m_iNextValidID++;
	m_pGroupState = new StateMachine<Group>(this);
	m_pGroupState->SetCurrentState(Idle::Instance());
	m_pGroupState->SetGlobalState(NULL);

	m_FormationID = 0;
	m_NumberUnits = 0;

}



Group::~Group()
{
	NullDelete(m_pGroupState);
}

void Group::Move(Vector2D destination)
{
	SetLeader();

	if(m_pLeader == NULL)
		return;

	Dispatcher->DispatchMessage(SENDER_IRRELEVANT, Leader()->ID(), MoveToLocation, &destination);

	CalculateOffsets();
	list<Vector2D>::iterator CurOffset = m_lOffsets.end(); CurOffset--;
	MemberList::iterator CurMember = m_Members.begin();

	CurMember++;
	while(CurMember != m_Members.end())
	{
		Vector2D dest = PointToWorldSpace(*CurOffset, Vector2D(0.0, 1.0), Vector2D(1.0, 0.0), destination);
		Dispatcher->DispatchMessage(SENDER_IRRELEVANT, (*CurMember)->ID(), MoveToLocation, &dest);
		CurOffset--;
		CurMember++;
	}
}

// Sets the leader
void Group::SetLeader()
{
	if(m_Members.empty())
	{
		m_pLeader = NULL;
		Log->AddMessage("ExitingSet Leader");
		return;
	}
	list<Character*>::iterator closest = m_Members.begin();
	list<Character*>::iterator comp = m_Members.begin(); comp++;

	for(comp; comp != m_Members.end(); ++comp)
	{
		if(ManhattanDist((*comp)->Pos(), m_MoveGoal) < ManhattanDist((*closest)->Pos(), m_MoveGoal))
			closest = comp;
	}

	m_pLeader = *closest;

	// Now we need to swap the elements so the leader is at the front of the list.
	Character* temp = *closest;
	*closest = m_Members.front();
	m_Members.front() = temp;
}


void Group::UpdateUnitPositions()
{
	if(m_Members.size() == 0)
		return;  // fixes that list error on game startup

	int i = 0;
	list<Character*>::iterator iter = m_Members.begin();


	for(iter; iter != m_Members.end(); iter++)
	{
		mUnitPositions[i] = (*iter)->Pos();
		++i;
	}
}


void Group::Add(Character* aChar)
{
	aChar->AddToGroup(m_ID);

	list<Character*>::iterator iter = m_Members.begin();
	bool alreadyHere = false;

	for(iter; iter != m_Members.end(); iter++)
	{
		if(*iter == aChar)
		{
			alreadyHere = true;
			break;
		}
	}

	if(!alreadyHere)
	{
		m_Members.push_back(aChar);
	}

	// The leader always has to be somebody. This is a temporary solution until I figure out
	// another way to set this up.
	m_pLeader = aChar;

	mUnitPositions.push_back(aChar->Pos());
	mDesiredPositions.resize(m_Members.size());
}

void Group::Remove(Character* aChar)
{
	m_Members.remove(aChar);
	aChar->RemoveFromGroup();

	mUnitPositions.resize(m_Members.size());
	UpdateUnitPositions();
}



void Group::Update()
{
	UpdateUnitPositions();
	m_pGroupState->Update();
	//CalculateCentroid();
}

bool Group::inGroup(Character* aChar)
{
	bool InGroup = false;
	list<Character*>::iterator iter = m_Members.begin();
	for(iter; iter != m_Members.end(); ++iter)
	{
		if(*iter = aChar)
			InGroup = true;
	}

	return InGroup;
}

void Group::CalculateOffsets()
{
	m_lOffsets.clear();
	switch(m_FormationID)
	{

		// Basic Formation
		case 0:
		{
			// Creates a box large enough to hold all except the leader of the group.
			list<Character*>::iterator iter = m_Members.begin(); 
			iter++; // Don't count the leader

			double area = 0;
			for(iter; iter != m_Members.end(); ++iter)
			{
				area += (*iter)->BRadius()*(*iter)->BRadius() + 3.14159;
			}
			
			double SideLength = sqrt(area) * 4.0;
			double increment = SideLength/sqrt((float) m_Members.size() - 1 ); // Again, don't count the leader.
			double leadDist = 50.0; // The distance the leader is from the group.

			Vector2D Offset;

			bool done = false; // We are done when we have enough offsets for all the characters in the group

			for(double Y = 0; true; Y += increment)
			{
				if(done)
					break;

				for(double X = 0; X <= SideLength; X += increment)
				{
					Offset = (Vector2D(0.0, SideLength/2.0) - Vector2D(X,Y)) + Vector2D(-leadDist, 0.0);
					
					m_lOffsets.push_back(Offset);
					
					if(m_lOffsets.size() == m_Members.size())
					{
						done = true;
						break;
					}
						
				}
			}
		}

	}

}

// -----------------NextFreeOffset----------------
// Returns the top offset on m_lOffsets and removes
// it from the list.

Vector2D Group::NextFreeOffset()
{
	Vector2D Offset = m_lOffsets.front();
	m_lOffsets.pop_front();
	
	return Offset;
}

// ------------------Members()-----------------
// Returns a list of all the characters currently
// in this group

MemberList Group::Members()
{
	return m_Members;
}


void Group::CalculateCentroid()
{
	Vector2D average;

	vector<Vector2D>::iterator iter = mUnitPositions.begin();
	while(iter != mUnitPositions.end())
	{
		average += *iter;
		iter++;
	}

	average /= mUnitPositions.size();

	m_Centroid = average;
}

void Group::Render()
{
}